#include "log.h"

extern rotate_speed_c rev;

static int current_cnt_in_profile_plus_one()
{
	int ret, fd, cnt;
	char cnt_write[5];
	cnt = get_current_cnt_in_profile();
	fd = open("/home/ubuntu/work/run_dir/test-cnt-log", O_RDWR);
	if(fd == -1)
	{
		printf("test-cnt-log open error.\n");
	}
	cnt++;
	sprintf(cnt_write, "%d" , cnt);
	ret = write(fd, &cnt_write, 1);
	if(ret == -1)
	{
		perror("write");
	}
	return 0;
}

xy_log_c::
xy_log_c(autopilot_interface_c &_pilot, ultrasonic_c &_xy_ultra, xy_vision_c &_xy_vision, ref_navigator_c &_ref_navigator_current,controller_3D_posPID_c &_controller_current):
	pilot(_pilot),xy_ultra(_xy_ultra),xy_vision(_xy_vision),ref_navigator_current(_ref_navigator_current), controller_current(_controller_current)
{}

xy_log_c::
~xy_log_c()
{}

int
xy_log_c::
init_log(int *fd)
{
	DIR* dir=opendir("/mnt/sdcard/log/");
	if(sem_init(&log_start_sem, 0, 0) != 0)
	printf("log_start_sem has been existing");

	if(sem_init(&log_ctrl_data_sem, 0, 0) != 0)
	printf("log_ctrl_data_sem has been existing");

	current_cnt_in_profile_plus_one();

	if(NULL == dir)
	{
		sprintf(log_name, "/home/ubuntu/work/Xunyi/log/sd-log-%d", get_current_cnt_in_profile() );
	}

	else
	{
		sprintf(log_name, "/mnt/sdcard/log/sd-log-%d", get_current_cnt_in_profile() );
	}

	printf("log_name is %s\n", log_name);

	*fd = open(log_name, O_RDWR | O_CREAT | O_APPEND, 0644);

	if(*fd == -1)
	{
		printf("Open or create log file failed, maybe no sd card.\n");
		goto release;
	}

	return 0;

release:
	memset(log_name, 0, strlen((const char *)log_name));
	return 1;
}

int
xy_log_c::
XY_SD_Log_Setup(void)
{
	if(init_log(&log_fd) != 0)
	{
		log_fd = -1;
		printf("log_fd is -1.\n");
		return -1;
	}
	if(XY_Create_Thread(start_store_to_log_thread_func, this, THREADS_LOG, -1, SCHED_RR, 89) < 0)
	{
		printf("Create Sd store Status Thread Error.\n");
		return -1;
	}

	return 0;
}

void
xy_log_c::
store_depend_stat(int _stat, char *strp)
{
	switch(_stat)
	{

		case 0x01:
			sprintf(strp, "%.8f,%.8f,%.8f,%d,%d,%.2f;", (float)log_GPS_position_part1.lat/1E7,
														(float)log_GPS_position_part1.lon/1E7,
														(float)log_GPS_position_part1.alt/1E7, log_GPS_position_part2.eph,
														log_GPS_position_part2.satellites_visible,
														(float)log_GPS_position_part1.relative_alt/1000);
			break;
		case 0x02:
			sprintf(strp, "%.2f,%.2f,%.2f,%.2f,%.2f,%.2f;", log_local_position.x,
															log_local_position.y,
															log_local_position.z,
															log_local_position.vx,
															log_local_position.vy,
															log_local_position.vz);
			break;

		case 0x04:
			sprintf(strp, "%.4f,%.4f,%.4f,%.4f,%.4f,%.4f;", log_attitude.roll,
															log_attitude.pitch,
															log_attitude.yaw,
															log_attitude.rollspeed,
															log_attitude.pitchspeed,
															log_attitude.yawspeed);
			break;

		case 0x08:
			sprintf(strp, "%d,%d,%d,%d,%d,%d;", log_system_status_part1.base_mode,
												log_system_status_part1.custom_mode,
												log_system_status_part1.system_status,
												log_system_status_part2.landed_state,
												log_system_status_part3.battery_remaining,
												log_system_status_part3.voltage_battery);
			break;

		case 0x10:
			sprintf(strp, "[%.2f,%.2f,%.2f,%.2f,%.2f],[%.2f,%.2f,%.2f,%.2f,%.2f],[%.2f,%.2f,%.2f,%.2f];",	log_controller_ref_point.position.x,
																								log_controller_ref_point.position.y,
																								log_controller_ref_point.position.z,
																								log_controller_ref_point.position.speed,
																								log_controller_ref_point.yaw,
																								log_controller_ref_feedback.position.x,
																								log_controller_ref_feedback.position.y,
																								log_controller_ref_feedback.position.z,
																								log_controller_ref_feedback.position.speed,
																								log_controller_ref_feedback.yaw,
																								log_d3_control.velocity.vx,
																								log_d3_control.velocity.vy,
																								log_d3_control.velocity.vz,
																								log_d3_control.yaw_ref);
			break;

        case 0x20:
			sprintf(strp, "%.2f,%.2f,%.2f,%.2f;", log_vision_position.x, log_vision_position.y, log_vision_position.z, log_vision_height);
			break;

		case 0x40:
			sprintf(strp, "%.2f,%.2f;", log_ultra_data.data, log_filtered_ultra_data);
			break;

		case 0x80:
			sprintf(strp, "%d,%d, %d;", log_gpio21_rev_speed, log_gpio18_rev_speed, log_chan3_raw);
			break;

		case 0x0100:
			sprintf(strp, "%u, %u, %u, %u, %u, %u, %u, %u;", 	log_servo_output_raw.servo1_raw,
																log_servo_output_raw.servo2_raw,
																log_servo_output_raw.servo3_raw,
																log_servo_output_raw.servo4_raw,
																log_servo_output_raw.servo5_raw,
																log_servo_output_raw.servo6_raw,
																log_servo_output_raw.servo7_raw,
																log_servo_output_raw.servo8_raw);
			break;

	}
	int ret = write(log_fd, strp, strlen((const char *)strp));
	memset(strp, 0, strlen((const char *)strp));
	if(ret == -1)
	{
		printf("Bad write.\n");
	}
}


void
xy_log_c::
check_stat_store(int _stat, int _mask, char *strp)
{
	if(_stat & _mask)
		store_depend_stat(_mask, strp);
	else	//no valid data
	{
		int ret = write(log_fd, ";", 1);
		if(ret == -1)
		{
			printf("Bad write.\n");
		}
	}
}

void
xy_log_c::
add_crlf(void)
{
	int ret = 0;
	ret = write(log_fd, CRLF, 1);
	if(ret == -1)
	{
		perror("write");
	}
}

void
xy_log_c::
set_log_on_flag(int _val)
{
	_val = _val > 1 ? 1 : _val;
	pthread_mutex_lock(&log_on_off_lock);
	log_on_flag = _val;
	pthread_mutex_unlock(&log_on_off_lock);
}

void
xy_log_c::
clear_log_on_flag(void)
{
	set_log_on_flag(0);
}

int
xy_log_c::
get_log_on_flag(void)
{
	int ret;
	pthread_mutex_lock(&log_on_off_lock);
	ret = log_on_flag;
	pthread_mutex_unlock(&log_on_off_lock);

	return ret;
}

int
xy_log_c::
if_log_on(void)
{
	return get_log_on_flag();
}

void
xy_log_c::
wait_log_on(void)
{
	sem_wait(&log_start_sem);
}


int
xy_log_c::
XY_Start_Store_Log(void)
{
	set_log_on_flag(1);
	//sem_post(&log_start_sem);
	int ret = rev.setup();
	printf("=========open rev=======\n");
	if(ret == -1)
	{
		printf("rotate_speed setup error\n");
	}
	return 0;
}

int
xy_log_c::
XY_Stop_Store_Log(void)
{
	set_log_on_flag(0);

	return 0;
}


// ------------------------------------------------------------------------------
//   log Thread
// ------------------------------------------------------------------------------
void
xy_log_c::
store_log_thread()
{
	int stat = 0;
	unsigned int log_cnt = 0;

	struct timeval	  tv;
	struct tm		  *tmlocal;
	char _wbuf[3000] = {0};

	if(log_fd == -1)
	{
		printf("Bad log fd.\n");
		pthread_exit(NULL);
	}

	char header[500];
	sprintf(header,
			"linux time;"
			"local time;"
			"system_id;"
			"lat,lon,alt,eph,satellites_visible,relative_alt;"
			"x,y,z,vx,vy,vz;"
			"roll,pitch,yaw,rollspeed,pitchspeed,yawspeed;"
			"base_mode,custom_mode,system_status,extended_sys_state.landed_state,battery_remaining,voltage_battery;"
			"controller.ref,controller.feedback,controller.3d_control;"
			"vision.data;"
			"ultra.data,filtered.data;"
			"rev_speed21,rev_speed18,chan3_raw;"
			"s1,s2,s3,s4,s5,s6,s7,s8;\n");

	int ret = write(log_fd, header, strlen((const char *) header));
	if(ret == -1)
	{
		printf("Header bad write.\n");
	}

	while(1)
	{
		if(if_log_on() )
		{
			log_GPS_position_part1 = pilot.current_messages.global_position_int;
			log_GPS_position_part2 = pilot.current_messages.gps_raw_int;
			stat |= 0x01;

			log_local_position = pilot.current_messages.local_position_ned;
			stat |= 0x02;

			log_attitude = pilot.current_messages.attitude;
			stat |= 0x04;

			log_system_status_part1 = pilot.current_messages.heartbeat;
			log_system_status_part2 = pilot.current_messages.extended_state;
			log_system_status_part3 = pilot.current_messages.sys_status;
			stat |= 0x08;

			log_controller_ref_point = controller_current.ref;
			log_controller_ref_feedback = controller_current.feedback;
			log_d3_control = controller_current.d3_ctrl;
			stat |= 0x10;

			log_vision_position = xy_vision.position;
			log_vision_height = xy_vision.vision_height;
			stat |= 0x20;

			log_ultra_data = xy_ultra.ultra_data;
			log_filtered_ultra_data = xy_ultra.filtered_data;
			stat |= 0x40;

			log_gpio21_rev_speed = get_gpio21_rev_speed();
			log_gpio18_rev_speed = get_gpio18_rev_speed();
			log_chan3_raw = pilot.current_messages.rc_channel.chan3_raw;
			stat |= 0x80;

			log_servo_output_raw = pilot.current_messages.servo_output_raw;
			stat |= 0x0100;
			if( gettimeofday(&tv, NULL) != 0)
			{
				printf("ERROR get timestamp for log.\n");

			}
			tmlocal = localtime(&tv.tv_sec);

			sprintf(_wbuf, "%d-%d-%d-%6ld;", 	tmlocal->tm_hour,
												tmlocal->tm_min,
												tmlocal->tm_sec,
												tv.tv_usec);
			int ret = write(log_fd, _wbuf, strlen((const char *)_wbuf));
			if(ret == -1)
			{
				perror("log write");
			}

			memset(_wbuf, 0, strlen((const char *)_wbuf));
			sprintf(_wbuf, "%.1f;", 0.1 * log_cnt++);
			ret = write(log_fd, _wbuf, strlen((const char *)_wbuf));
			if(ret == -1)
			{
				perror("log write");
			}

			memset(_wbuf, 0, strlen((const char *)_wbuf));
			sprintf(_wbuf, "%d;", XYI_UAV_ID);
			ret = write(log_fd, _wbuf, strlen((const char *)_wbuf));
			if(ret == -1)
			{
				perror("log write");
			}

			memset(_wbuf, 0, strlen((const char *)_wbuf));
			check_stat_store(stat, 0x01, _wbuf);
			check_stat_store(stat, 0x02, _wbuf);
			check_stat_store(stat, 0x04, _wbuf);
			check_stat_store(stat, 0x08, _wbuf);
			check_stat_store(stat, 0x10, _wbuf);
			check_stat_store(stat, 0x20, _wbuf);
			check_stat_store(stat, 0x40, _wbuf);
			check_stat_store(stat, 0x80, _wbuf);
			check_stat_store(stat, 0x0100, _wbuf);

			add_crlf();

			stat = 0;

			usleep(100000); //100ms
		}
		else
		{
			printf("log fd will be closed.\n");
			close(log_fd);
			break;
			//wait_log_on();
		}
	}
	pthread_exit(NULL);

	return;
}


// ------------------------------------------------------------------------------
//  Pthread Starter Helper Functions
// --
void*
start_store_to_log_thread_func(void *args)
{
	// takes an autopilot object argument
	xy_log_c *xy_log = (xy_log_c *)args;

	// run the object's read thread
	xy_log->store_log_thread();

	// done!
	return NULL;
}

